10. Errors

We can capture how the errors we are interested in change over time by deriving our kinematic model around these errors as our new state vector.

The new state is [x, y, \psi, v, cte, e\psi] .

Let’s assume the vehicle is traveling down a straight road and the longitudinal direction is the same as the x-axis.

Cross Track Error

We can express the error between the center of the road and the vehicle's position as the cross track error (CTE). The CTE of the successor state after time t is the state at t + 1, and is defined as:

cte_{t+1} = cte_t + v_t* sin(e\psi_t) * dt

In this case cte_t can be expressed as the difference between the line and the current vehicle position y . Assuming the reference line is a 1st order polynomial f , f(x_t) is our reference line and our CTE at the current state is defined as:

cte_t = f(x_t) - y_t

If we substitute cte_t back into the original equation the result is:

cte_{t+1} = f(x_t) - y_t + (v_t * sin(e\psi_t) * dt)

This can be broken up into two parts:

1. f(x_t) - y_t being current cross track error.
2. v_t * sin(e\psi_t) * dt being the change in error caused by the vehicle's movement.

Orientation Error

Ok, now let’s take a look at the orientation error:

The update rule is essentially the same as \psi .

e\psi_t is the desired orientation subtracted from the current orientation:

We already know \psi_t , because it’s part of our state. We don’t yet know \psi{des}_t (desired psi) - all we have so far is a polynomial to follow. \psi{des}_t can be calculated as the tangential angle of the polynomial f evaluated at x_t , arctan(f'(x_t)) . f' is the derivative of the polynomial.

Similarly to the cross track error this can be interpreted as two parts:

1. \psi_t - \psi{des}_t being current orientation error.
2. \frac{v_t} { L_f} * \delta_t * dt being the change in error caused by the vehicle's movement.